/*********************************************************************************
* @file         sensor_uart.c
* @ingroup      userapp
* @brief        获取陀螺仪姿态数据
* @attention
* @note         
* @author       WP              
**********************************************************************************/

#include  "zos.h"

#ifdef BOARD_USING_UART_DEMO

static int crcsensormsg(uint8_t *data);

//任务全局变量
char grebuf[UARTLEN];
double gcalmsg[10];

static zos_task_t user_uart_task_Handle = NULL;
static struct attitude_msg_cal calmsg;
static zos_uint8_t cmd_set[3];

//角度初始化，使Z轴角度归零
void send_cmd_w_init(void)
{
	cmd_set[0] = 0xFF;
	cmd_set[1] = 0xAA;
	cmd_set[2] = 0x52;
	zos_uart_write(ZOS_UART_BUS_1,cmd_set,sizeof(cmd_set));
}

//加速度计校准，校准加速度零偏
void send_cmd_a_init(void)
{
	cmd_set[0] = 0xFF;
	cmd_set[1] = 0xAA;
	cmd_set[2] = 0x67;
	zos_uart_write(ZOS_UART_BUS_1,cmd_set,sizeof(cmd_set));
}

//休眠，进入待机模式
void send_cmd_sleep_set(void)
{
	cmd_set[0] = 0xFF;
	cmd_set[1] = 0xAA;
	cmd_set[2] = 0x60;
	zos_uart_write(ZOS_UART_BUS_1,cmd_set,sizeof(cmd_set));
}

//唤醒待机状态
void send_cmd_wake_set(void)
{
	cmd_set[0] = 0xFF;
	cmd_set[1] = 0x11;
	cmd_set[2] = 0x11;
	zos_uart_write(ZOS_UART_BUS_1,cmd_set,sizeof(cmd_set));
}

//垂直安装，模块垂直放置
void send_cmd_vertical_set(void)
{
	cmd_set[0] = 0xFF;
	cmd_set[1] = 0xAA;
	cmd_set[2] = 0x66;
	zos_uart_write(ZOS_UART_BUS_1,cmd_set,sizeof(cmd_set));
}

//水平安装，模块水平放置
void send_cmd_level_set(void)
{
	cmd_set[0] = 0xFF;
	cmd_set[1] = 0xAA;
	cmd_set[2] = 0x65;
	zos_uart_write(ZOS_UART_BUS_1,cmd_set,sizeof(cmd_set));
}


//串口中断回调函数
//sprintf(grebuf,"%s",rev_data);取出的数据不对不完整
//strcpy(grebuf,(const char *)rev_data);
void uart_callback(zos_uint8_t *rev_data, zos_uint16_t len)
{
	if(len==UARTLEN)
		len=UARTLEN;
	if(crcsensormsg(rev_data)!=0)
		return;
	for(int i=0;i<UARTLEN;i++)
	{
		grebuf[i] = rev_data[i];
	}
}

//计算数据
static void calsensormsg(char *data)
{
	zos_uint8_t t51=0,t52=0;
	memset(&calmsg,0,sizeof(struct attitude_msg_cal));
	if(data[0]==0x55&&data[1]==0x51)
	{
		calmsg.ax = ((double)(data[3]<<8|data[2]))/32768*16;
		calmsg.ay = ((double)(data[5]<<8|data[4]))/32768*16;
		calmsg.az = ((double)(data[7]<<8|data[6]))/32768*16;
		t51 = ((double)(data[9]<<8|data[8]))/126.8+25;
	}

	if(data[11]==0x55&&data[12]==0x52)
	{
		calmsg.wx = ((double)(data[14]<<8|data[13]))/32768*2000;
		calmsg.wy = ((double)(data[16]<<8|data[15]))/32768*2000;
		calmsg.wz = ((double)(data[18]<<8|data[17]))/32768*2000;
		t52 = ((double)(data[20]<<8|data[19]))/126.8+25;
	}

	if(data[22]==0x55&&data[23]==0x53)
	{
		calmsg.roll = ((double)(data[25]<<8|data[24]))/32768*180;
		calmsg.pitch = ((double)(data[27]<<8|data[26]))/32768*180;
		calmsg.yaw = ((double)(data[29]<<8|data[28]))/32768*180;
		if(t51!=t52)
			calmsg.t = ((double)(data[31]<<8|data[30]))/126.8+25;
		calmsg.t = t51;
	}
}

//校验数据
static int crcsensormsg(uint8_t *data)
{
	uint8_t i=0;
	int32_t sum=0,err=0;
	if(data[0]==0x55&&data[1]==0x51)
	{
		for(i=0; i<10; i++)
		{
			sum += data[i];
		}
		sum = sum & 0xff;
		if(sum!=data[10])
		{
			err=0x51;
			zos_printf("0x51 crccal err!\r\n");
		}	
	}
	sum=0;
	if(data[11]==0x55&&data[12]==0x52)
	{
		for(i=11; i<21; i++)
		{
			sum += data[i];
		}
		sum = sum & 0xff;
		if(sum!=data[21])
		{
			err=0x52;
			zos_printf("0x52 crccal err!\r\n");
		}
	}
	sum=0;
	if(data[22]==0x55&&data[23]==0x53)
	{
		for(i=22; i<32; i++)
		{
			sum += data[i];
		}
		sum = sum & 0xff;
		if(sum!=data[32])
		{
			err=0x53;
			zos_printf("0x53 crccal err!\r\n");
		}
	}

	return err;
}

//uart task
static void sensor_mpu6050(void)
{
	ZOS_UART_CONFIG uart_cfg;
	uart_cfg.baud_rate=BTL9600;
	uart_cfg.data_bits=ZOS_UART_DATA_BITS_8;
	uart_cfg.parity=ZOS_UART_PARITY_NONE;
	uart_cfg.stop_bits=ZOS_UART_STOP_BITS_1;
	zos_uart_init(ZOS_UART_BUS_1,&uart_cfg,uart_callback);

	send_cmd_wake_set();

	while(1)
	{
		osDelay(1000);
	}

	user_uart_task_Handle = NULL;
	osThreadExit();
}

void sensor_mpu6050_task(void)
{
	user_uart_task_Handle = osThreadNew ((osThreadFunc_t)(sensor_mpu6050),NULL,"sensor_mpu6050",0x400,osPriorityNormal1);;
}


#include "shell.h"

SHELL_EXPORT_CMD(send_cmd_w_init,send_cmd_w_init, cmd wset);//角度初始化，使Z轴角度归零
SHELL_EXPORT_CMD(send_cmd_a_init,send_cmd_a_init, cmd aset);//加速度计校准，校准加速度零偏
SHELL_EXPORT_CMD(send_cmd_sleep_set,send_cmd_sleep_set, cmd sleepset);//休眠，进入待机模式
SHELL_EXPORT_CMD(send_cmd_wake_set,send_cmd_wake_set, cmd wake);//唤醒待机状态
SHELL_EXPORT_CMD(send_cmd_vertical_set,send_cmd_vertical_set, cmd vertical);//垂直安装，模块垂直放置
SHELL_EXPORT_CMD(send_cmd_level_set,send_cmd_level_set, cmd level);//水平安装，模块水平放置

#endif //BOARD_USING_UART_DEMO
